﻿//===========================[ 6 / 19 / 2011 LVI ]=============================
// 								Header file
// Purpose: MathLib combination 2 source mathematics libs
// [1] dhpoware : http://www.dhpoware.com/source/mathlib.html
// [3] math lib from NVIDIA Graphics SDK
// Modified by Vitaly Lyaschenko <SkyChameleon@gmail.com>
//=============================================================================
#pragma once

#include <cmath>
#include <memory.h>
#include <cstdlib>

#define PI         3.14159265358979323846f
#define HALF_PI    PI / 2.0f
#define QUARTER_PI PI / 4.0f
#define TWO_PI     PI * 2.0f
#define EPSILON    1e-6f

#define RADIANS PI / 180.0f
#define DEGRESS 180.0f / PI

// classes 
class vec2;
class vec3;
class vec4;
class mat3;
class mat4;
class Quaternion;

//--------------------------
// FUNCTIONS 2D Vector
//--------------------------

float distance(const vec2 &pt1, const vec2 &pt2);

float distanceSq(const vec2 &pt1, const vec2 &pt2);

float dot(const vec2 &p, const vec2 &q);

vec2 lerp(const vec2 &p, const vec2 &q, float t);

void orthogonalize(vec2 &v1, vec2 &v2);

vec2 proj(const vec2 &p, const vec2 &q);

vec2 perp(const vec2 &p, const vec2 &q);

vec2 reflect(const vec2 &i, const vec2 &n);

float length(const vec2 &v);

float lengthSq(const vec2 &v);

vec2 invert(const vec2 v);

void normalize(vec2 &v);

void set(vec2 &v, float x_, float y_);

//--------------------------
// FUNCTIONS 3D Vector
//--------------------------

vec3 cross(const vec3 &p, const vec3 &q);

float distance(const vec3 &pt1, const vec3 &pt2);

float distanceSq(const vec3 &pt1, const vec3 &pt2);

float dot(const vec3 &p, const vec3 &q);

vec3 lerp(const vec3 &p, const vec3 &q, float t);

void orthogonalize(vec3 &v1, vec3 &v2);

void orthogonalize(vec3 &v1, vec3 &v2, vec3 &v3);

vec3 proj(vec3 &p, vec3 &q);

vec3 perp(const vec3 &p, const vec3 &q);

vec3 reflect(const vec3 &i, const vec3 &n);

float length(const vec3 &v);

float lengthSq(vec3 &v);

vec3 invert(const vec3 v);

vec3 normalize(const vec3 &v);

void normalize(vec3 &v);

void set(vec3 &v, float x_, float y_, float z_);

//--------------------------
// FUNCTIONS 4D Vector
//--------------------------

float distance(const vec4 &pt1, const vec4 &pt2);

float distanceSq(const vec4 &pt1, const vec4 &pt2);

float dot(const vec4 &p, const vec4 &q);

vec4 lerp(const vec4 &p, const vec4 &q, float t);

float length(const vec4 v);

float lengthSq(const vec4 v);

vec4 invert(const vec4 v);

void normalize(vec4 &v);

void set(vec4 &v, float x_, float y_, float z_, float w_);

void set(const vec2 &from, vec4 &to);

void set(const vec3 &from, vec4 &to);

void set(const vec4 &from, vec4 &to);

//--------------------------
// FUNCTIONS row-major 3x3 MATRIX
//--------------------------

float determinant(mat3 M);

void fromAxes(mat3 &M, const vec3 &x, const vec3 &y, const vec3 &z);

void fromAxesTransposed(mat3 &M, const vec3 &x, const vec3 &y, const vec3 &z);

void identity(mat3 &M);

mat3 invert(const mat3& A);

mat3 orient(const vec3 &from, const vec3 &to);

void rotate(mat3 &M, const vec3 &axis, float degrees);

void scale(mat3 &M, float sx, float sy, float sz);

void scale(mat3 &M, const vec3 v);

void toAxes(mat3 &M, vec3 &x, vec3 &y, vec3 &z);

void toAxesTransposed(mat3 &M, vec3 &x, vec3 &y, vec3 &z);

mat3 toHeadPitchRoll(float &headDegrees, float &pitchDegrees, float &rollDegrees);

mat3 transpose(mat3 M);

//--------------------------
// FUNCTIONS row-major 4x4 MATRIX
//--------------------------

float determinant(mat4 M);

void fromAxes(mat4 &M, const vec3 &x, const vec3 &y, const vec3 &z);

void fromAxesTransposed(mat4 &M, const vec3 &x, const vec3 &y, const vec3 &z);

void identity(mat4 &M);

void toAxes(mat4 &M, vec3 &x, vec3 &y,vec3 &z);

mat4 &invert(mat4& B, const mat4& M);

mat4 invert_rot_trans( const mat4& M );

void orient(mat4 &M, const vec3 &from, const vec3 &to);

mat4 rotate( const vec3 &axis, float degrees);

void rotate(mat4 &M, const vec3 &axis, float degrees);

void rotateX( mat4 &M, float angle );

mat4 rotateX( float angle );

void rotateY( mat4 &M, float angle );

mat4 rotateY( float angle );

void rotateZ( mat4 &M, float angle );

mat4 rotateZ( float angle );

mat4 scale(vec3 v);

mat4 scale(float sx, float sy, float sz);

void toAxesTransposed(mat4 &M, vec3 &x, vec3 &y, vec3 &z);

mat4 transpose(const mat4 &M);

void perspective(mat4 &M, float fov, float aspect, float zNear, float zFar);

const mat4 perspective(float fov, float aspect, float zNear, float zFar);

void orthographic(mat4 &m, float left, float right, float bottom, float top, float zNear, float zFar);

mat4 orthographic(float left, float right, float bottom, float top, float zNear, float zFar);

mat4 lookAt(const vec3 &position, const vec3 &center, const vec3 &up);

const mat4 fromEuler(float x, float y, float z);

const mat4 fromEuler(const vec3 &r);

const vec3 toEuler(const mat4 &M);

const mat4 translation(float x, float y, float z);

const mat4 translation(const vec3 &t);

//--------------------------
// Quaternion
//--------------------------


//-------------------------- 
// Misc
//-------------------------- 

template <typename T>
T bilerp(const T &a, const T &b, const T &c, const T &d, float u, float v);

void cartesianToSpherical(float x, float y, float z, float &rho, float &phi, float &theta);

bool closeEnough(float f1, float f2);

float degreesToRadians(float degrees);

float radiansToDegrees(float radians);

long floatToLong(float f);

bool isPower2(int x);

float lerp(float t, const float a, const float b );

int nextMultipleOf(int multiple, int value);

int nextPower2(int x);

float random(float min, float max);

void sphericalToCartesian(float rho, float phi, float theta, float &x, float &y, float &z);

float smoothstep(float a, float b, float x);

//-----------------------------------------------------------------------------
// A 2-component vector class that represents a row vector.

class vec2
{
public:
	union
	{
		struct { float x, y; }; 
		struct { float u, v; }; // текстурн. коор.
		float v_arr[2];
	};

	vec2() : x(1), y(1) {}
	vec2(const vec2& v) : x(v.x), y(v.y) {}
	vec2(const vec3& v);
	vec2(const vec4& v);
	vec2(const float *f) : x(f[0]), y(f[1]) {}
	vec2(const float x_, const float y_) : x(x_), y(y_) {}
	vec2(const float fv) : x(fv), y(fv) {}
	~vec2() {}

	bool operator==(const vec2 &v) const
	{
		return (v.x == x && v.y == y) ? true : false;
	}

	bool operator!=(const vec2 &v) const
	{
		return !(*this == v );
	}

	vec2 & operator*=(const float &l)
	{
		x*= l;
		y*= l;
		return *this;
	}

	vec2 & operator+=(const vec2 &u)
	{
		x+= u.x;
		y+= u.y;
		return *this;
	}

	vec2 & operator-=(const vec2 &u)
	{
		x-= u.x;
		y-= u.y;
		return *this;
	}

	vec2 & operator/=(float scalar)
	{
		x /= scalar, y /= scalar;
		return *this;
	}
};

inline const vec2 operator+(const vec2 &u, const vec2 &v)
{
	return vec2(u.x + v.x, u.y + v.y);
}

inline const vec2 operator-(const vec2 &u, const vec2 &v)
{
	return vec2(u.x - v.x, u.y - v.y);
}

inline const vec2 operator*(const vec2 &u, const vec2 &v)
{
	return vec2(u.x * v.x, u.y * v.y);
}

inline const vec2 operator*(const float s, const vec2 &u)
{
	return vec2(s * u.x, s * u.y);
}

inline const vec2 operator/(const vec2 &u, const float s)
{
	return vec2(u.x / s, u.y / s);
}

//-----------------------------------------------------------------------------
// A 3-component vector class that represents a row vector.

class vec3
{
public:
	union
	{
		struct { float x, y, z; };
		float v_arr[3];
	};

	vec3() : x(1), y(1), z(1) {}
	vec3(const float fv): x(fv), y(fv), z(fv) {}
	vec3(const vec2 &v)	: x(v.x), y(v.y), z(1.0) {}
	vec3(const vec3 &v)	: x(v.x), y(v.y), z(v.z) {}
	vec3(const vec4 &v);
	vec3(const float *f): x(f[0]), y(f[1]), z(f[2]) {}
	vec3(const float x_, const float y_, const float z_) : x(x_), y(y_), z(z_) {}
	~vec3() {}

	//bool operator==(const vec3 &u) const
	//{
	//	return (u.x == x && u.y == y && u.z == z) ? true : false;
	//}

	//bool operator!=(const vec3 &rhs) const
	//{
	//	return !(*this == rhs );
	//}

	vec3 & operator+=(const vec3 &u)
	{
		x+= u.x;
		y+= u.y;
		z+= u.z;
		return *this;
	}

	vec3 & operator-=(const vec3 &u)
	{
		x-= u.x;
		y-= u.y;
		z-= u.z;
		return *this;
	}

	vec3 & operator*=(const float & _l)
	{
		x*= _l;
		y*= _l;
		z*= _l;
		return *this;
	}

	vec3 operator-() const
	{
		return vec3(-x, -y, -z);
	}

	void operator()(const vec3 &v) 
	{
		x = v.x; 
		y = v.y; 
		z = v.z;
	}

	void operator()(float x_, float y_, float z_) 
	{
		x = x_; 
		y = y_; 
		z = z_;
	}

	float & operator[](int i)
	{
		return v_arr[i];
	}

	const float operator[](int i) const
	{
		return v_arr[i];
	}
};

inline bool operator==(const vec3 &u, const vec3 &v)
{
	return (u.x == v.x) && (u.y== v.y) && (u.z == v.z);
}

inline bool operator!=(const vec3 &u, const vec3 &v)
{
	return !(u == v );
}

inline bool operator<(const vec3 &u, const vec3 &v)
{
	return (u.x < v.x) && (u.y < v.y) && (u.z < v.z);
}

inline const vec3 operator+(const vec3 &u, const vec3 &v)
{
	return vec3(u.x + v.x, u.y + v.y, u.z + v.z);
}

inline const vec3 operator-(const vec3 &u, const vec3 &v)
{
	return vec3(u.x - v.x, u.y - v.y, u.z - v.z);
}

inline const vec3 operator*(const vec3 &u, const vec3 &v)
{
	return vec3(u.x * v.x, u.y * v.y, u.z * v.z);
}

inline const vec3 operator*(const float s, const vec3 &u)
{
	return vec3(s * u.x, s * u.y, s * u.z);
}

inline const vec3 operator*(const vec3 &u, const float s )
{
	return vec3(u.x * s, u.y * s, u.z * s);
}

inline const vec3 operator/(const vec3 &u, const float s)
{
	return vec3(u.x / s, u.y / s, u.z / s);
}

inline vec2::vec2(const vec3& v) : x(v.x), y(v.y) 
{}

//-----------------------------------------------------------------------------
// A 4-component row vector class that represents a point or vector in 
// homogeneous coordinates.

class vec4
{
public:
	union
	{
		struct { float x, y, z, w; };
		float v_arr[4];
	};

	vec4() : x(0), y(0), z(0), w(0) {}
	vec4(const vec2 &v) : x(v.x), y(v.y), z(1.0), w(1.0) {}
	vec4(const vec3 &v) : x(v.x), y(v.y), z(v.z), w(1.0) {}
	vec4(const vec4 &v) : x(v.x), y(v.y), z(v.z), w(v.w) {}
	vec4(const float x_, const float y_, const float z_, const float w_) : x(x_), y(y_), z(z_), w(w_) {}
	vec4(const vec3 &v, const float w_) : x(v.x), y(v.y), z(v.z), w(w_) {}

	~vec4() {}

	bool operator==(const vec4 &u) const
	{
		return (u.x == x && u.y == y && u.z == z && u.w == w) ? true : false;
	}

	bool operator!=(const vec4 &u) const
	{
		return !(*this == u );
	}

	vec4 & operator+=(const vec4 &u)
	{
		x+= u.x;
		y+= u.y;
		z+= u.z;
		w+= u.w;
		return *this;
	}

	vec4 & operator-=(const vec4 &u)
	{
		x-= u.x;
		y-= u.y;
		z-= u.z;
		w-= u.w;
		return *this;
	}

	vec4 & operator*=(const float &_l)
	{
		x*= _l;
		y*= _l;
		z*= _l;
		w*= _l;
		return *this;
	}

	vec4 & operator/=(float scalar)
	{
		x /= scalar, y /= scalar, z /= scalar, w /= scalar;
		return *this;
	}

	float & operator[](int i)
	{
		return v_arr[i];
	}

	const float operator[](int i) const
	{
		return v_arr[i];
	}

	void  operator()(const vec4 &v)
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = v.w;
	}

	void  operator()(float x_, float y_, float z_, float w_)
	{
		x = x_;
		y = y_;
		z = z_;
		w = w_;
	}
};

inline const vec4 operator+(const vec4 &u, const vec4 &v)
{
	return vec4(u.x + v.x, u.y + v.y, u.z + v.z, u.w + v.w);
}

inline const vec4 operator-(const vec4 &u, const vec4 &v)
{
	return vec4(u.x - v.x, u.y - v.y, u.z - v.z, u.w - v.w);
}

inline const vec4 operator*(const vec4 &u, const vec4 &v)
{
	return vec4(u.x * v.x, u.y * v.y, u.z * v.z, u.w * v.w);
}

inline const vec4 operator*(const float s, const vec4 &u)
{
	return vec4(s * u.x, s * u.y, s * u.z, s * u.w);
}

inline const vec4 operator/(const vec4 &u, const float s)
{
	return vec4(u.x / s, u.y / s, u.z / s, u.w / s);
}

inline vec2::vec2(const vec4& v) : x(v.x), y(v.y) 
{}

inline  vec3::vec3(const vec4 &v) : x(v.x), y(v.y), z(v.z) 
{}
//-----------------------------------------------------------------------------
// A row-major 3x3 matrix class.
//
// Matrices are concatenated in a left to right order.
// Multiplies vectors to the left of the matrix.

class mat3
{
public:
	union
	{
		struct {
			float a0, a1, a2;        
			float a3, a4, a5;       
			float a6, a7, a8; 
		};
		float m_arr[9];
	};

	// constructors
	mat3()
		: a0(0.0f), a1(0.0f), a2(0.0f), 
		  a3(0.0f), a4(0.0f), a5(0.0f), 
		  a6(0.0f), a7(0.0f), a8(0.0f) {}

	mat3(const mat3 &M)
	{
		memcpy(m_arr, M.m_arr, sizeof(float) * 9);
	}

	mat3(const float *f)
	{
		memcpy(m_arr, f, sizeof(float) * 9);
	}

	mat3(const float& m0, const float& m1, const float& m2,
		 const float& m3, const float& m4, const float& m5,
		 const float& m6, const float& m7, const float& m8)
		: a0(m0), a1(m1), a2(m2), 
		  a3(m3), a4(m4), a5(m5), 
		  a6(m6), a7(m7), a8(m8) {}

	mat3(const mat4 &M);

	// operators
	bool operator==(const mat3 &M) const
	{
		return	 closeEnough(m_arr[0], M[0])
			&&	 closeEnough(m_arr[1], M[1])
			&&	 closeEnough(m_arr[2], M[2])
			&&	 closeEnough(m_arr[3], M[3])
			&&	 closeEnough(m_arr[4], M[4])
			&&	 closeEnough(m_arr[5], M[5])
			&&	 closeEnough(m_arr[6], M[6])
			&&	 closeEnough(m_arr[7], M[7])
			&&	 closeEnough(m_arr[8], M[8]);
	}

	bool operator!=(const mat3 &M) const
	{
		return !(*this == M);
	}

	mat3 & operator+=(const mat3 &M)
	{
		m_arr[0] += M[0]; m_arr[1] += M[1]; m_arr[2] += M[2];
		m_arr[3] += M[3]; m_arr[4] += M[4]; m_arr[5] += M[5];
		m_arr[6] += M[6]; m_arr[7] += M[7]; m_arr[8] += M[8];
		return *this;
	}

	mat3 & operator-=(const mat3 &M)
	{
		m_arr[0] -= M[0]; m_arr[1] -= M[1]; m_arr[2] -= M[2];
		m_arr[3] -= M[3]; m_arr[4] -= M[4]; m_arr[5] -= M[5];
		m_arr[6] -= M[6]; m_arr[7] -= M[7]; m_arr[8] -= M[8];
		return *this;
	}

	mat3 & operator*=(const mat3 &M)
	{
		m_arr[0] = ( m_arr[0] * M[0] + m_arr[1] * M[3] + m_arr[2] * M[6] );
		m_arr[1] = ( m_arr[0] * M[1] + m_arr[1] * M[4] + m_arr[2] * M[7] );
		m_arr[2] = ( m_arr[0] * M[2] + m_arr[1] * M[5] + m_arr[2] * M[8] );
		m_arr[3] = ( m_arr[3] * M[0] + m_arr[4] * M[3] + m_arr[5] * M[6] );
		m_arr[4] = ( m_arr[3] * M[1] + m_arr[4] * M[4] + m_arr[5] * M[7] );
		m_arr[5] = ( m_arr[3] * M[2] + m_arr[4] * M[5] + m_arr[5] * M[8] );
		m_arr[6] = ( m_arr[6] * M[0] + m_arr[7] * M[3] + m_arr[8] * M[6] );
		m_arr[7] = ( m_arr[6] * M[1] + m_arr[7] * M[4] + m_arr[8] * M[7] );
		m_arr[8] = ( m_arr[6] * M[2] + m_arr[7] * M[5] + m_arr[8] * M[8] );
		return *this;
	}

	mat3 & operator*=(float f)
	{
		m_arr[0] *= f; m_arr[1] *= f; m_arr[2] *= f;
		m_arr[3] *= f; m_arr[4] *= f; m_arr[5] *= f;
		m_arr[6] *= f; m_arr[7] *= f; m_arr[8] *= f;
		return *this;
	}

	mat3 & operator/=(float f)
	{
		m_arr[0] /= f; m_arr[1] /= f; m_arr[2] /= f;
		m_arr[3] /= f; m_arr[4] /= f; m_arr[5] /= f;
		m_arr[6] /= f; m_arr[7] /= f; m_arr[8] /= f;
		return *this;
	}

	const mat3 operator*(const mat3 &M) const
	{
		return mat3(m_arr[0] * M[0] + m_arr[1] * M[3] + m_arr[2] * M[6],
					m_arr[0] * M[1] + m_arr[1] * M[4] + m_arr[2] * M[7],
					m_arr[0] * M[2] + m_arr[1] * M[5] + m_arr[2] * M[8],
					m_arr[3] * M[0] + m_arr[4] * M[3] + m_arr[5] * M[6],
					m_arr[3] * M[1] + m_arr[4] * M[4] + m_arr[5] * M[7],
					m_arr[3] * M[2] + m_arr[4] * M[5] + m_arr[5] * M[8],
					m_arr[6] * M[0] + m_arr[7] * M[3] + m_arr[8] * M[6],
					m_arr[6] * M[1] + m_arr[7] * M[4] + m_arr[8] * M[7],
					m_arr[6] * M[2] + m_arr[7] * M[5] + m_arr[8] * M[8]);
	}

	mat3 operator+(const mat3 &rhs) const
	{
		mat3 tmp(*this);
		tmp += rhs;
		return tmp;
	}

	mat3 operator-(const mat3 &rhs) const
	{
		mat3 tmp(*this);
		tmp -= rhs;
		return tmp;
	}

	const mat3 operator*(float f) const
	{
		return mat3(m_arr[0] * f, m_arr[1] * f, m_arr[2] * f,
					m_arr[3] * f, m_arr[4] * f, m_arr[5] * f,
					m_arr[6] * f, m_arr[7] * f, m_arr[8] * f);
	}

	const mat3 operator/(float f) const
	{
		return mat3(m_arr[0] / f, m_arr[1] / f, m_arr[2] / f,
					m_arr[3] / f, m_arr[4] / f, m_arr[5] / f,
					m_arr[6] / f, m_arr[7] / f, m_arr[8] / f);
	}

	float operator[](int i)
	{
		return m_arr[i];
	}

	float operator[](int i) const
	{
		return m_arr[i];
	}

	void operator()(const mat3 &M)
	{
		memcpy(m_arr, M.m_arr, sizeof(float) * 9);
	}
};

inline const vec2 operator*(const mat3 &M, const vec2 &v)
{
	return vec2((M[0] * v.x) + (M[1] * v.y) + M[2],
				(M[3] * v.x) + (M[4] * v.y) + M[5]);
}

inline vec3 operator*(const vec3 &v, const mat3 &M)
{
	return vec3((v.x * M[0]) + (v.y * M[1]) + (v.z * M[2]),
				(v.x * M[3]) + (v.y * M[4]) + (v.z * M[5]),
				(v.x * M[6]) + (v.y * M[7]) + (v.z * M[8]));
}

inline vec3 operator*(const mat3 &M, const vec3 &v)
{
	return vec3((M[0] * v.x) + (M[1] * v.y) + (M[2] * v.z),
				(M[3] * v.x) + (M[4] * v.y) + (M[5] * v.z),
				(M[6] * v.x) + (M[7] * v.y) + (M[8] * v.z));
}

//-----------------------------------------------------------------------------
// A homogeneous row-major 4x4 matrix class.
//
// Matrices are concatenated in a left to right order.
// Multiplies vectors to the left of the matrix.

class mat4
{
public:
	union
	{
		struct {
			float  a0,  a1,  a2,  a3;  
			float  a4,  a5,  a6,  a7;   
			float  a8,  a9, a10, a11;  
			float a12, a13, a14, a15;  
		};
		float m_arr[16];
	};

	mat4() :  a0(1), a1(0), a2(0), a3(0),
		      a4(0), a5(1), a6(0), a7(0),
			  a8(0), a9(0),a10(1),a11(0),
			 a12(0),a13(0),a14(0),a15(1)
	{ }

	/*mat4(const float *f)
		: a0(f[ 0]), a1(f[ 1]), a2(f[ 2]), a3(f[ 3]),
		  a4(f[ 4]), a5(f[ 5]), a6(f[ 6]), a7(f[ 7]),
		  a8(f[ 8]), a9(f[ 9]),a10(f[10]),a11(f[11]),
		 a12(f[12]),a13(f[13]),a14(f[14]),a15(f[15]) {} */
	mat4(const float *f)
	{
		memcpy(m_arr, f, sizeof(float) * 16);
	}
	
	mat4(const mat3 &M)
		: a0(M[0]), a1(M[1]), a2(M[2]), a3(0),
		  a4(M[3]), a5(M[4]), a6(M[5]), a7(0),
		  a8(M[6]), a9(M[7]),a10(M[8]),a11(0),
		    a12(0),   a13(0),   a14(0),a15(1)
	{ }

	/*mat4(const mat4 &M)
		: a0(M[ 0]), a1(M[ 1]), a2(M[ 2]), a3(M[ 3]),
		  a4(M[ 4]), a5(M[ 5]), a6(M[ 6]), a7(M[ 7]),
		  a8(M[ 8]), a9(M[ 9]),a10(M[10]),a11(M[11]),
		 a12(M[12]),a13(M[13]),a14(M[14]),a15(M[15]) {}*/

	mat4(const mat4 &M)
	{
		memcpy(m_arr, M.m_arr, sizeof(float) * 16);
	}
	
	mat4(float  m0, float  m1, float  m2, float  m3,
		 float  m4, float  m5, float  m6, float  m7,
		 float  m8, float  m9, float m10, float m11,
		 float m12, float m13, float m14, float m15)
		: a0( m0), a1( m1), a2( m2), a3( m3),
		  a4( m4), a5( m5), a6( m6), a7( m7),
		  a8( m8), a9( m9),a10(m10),a11(m11),
		 a12(m12),a13(m13),a14(m14),a15(m15) {}

	~mat4() {}

	bool operator==(const mat4 &M) const
	{
		return closeEnough(m_arr[ 0], M[ 0])
			&& closeEnough(m_arr[ 1], M[ 1])
			&& closeEnough(m_arr[ 2], M[ 2])
			&& closeEnough(m_arr[ 3], M[ 3])
			&& closeEnough(m_arr[ 4], M[ 4])
			&& closeEnough(m_arr[ 5], M[ 5])
			&& closeEnough(m_arr[ 6], M[ 6])
			&& closeEnough(m_arr[ 7], M[ 7])
			&& closeEnough(m_arr[ 8], M[ 8])
			&& closeEnough(m_arr[ 9], M[ 9])
			&& closeEnough(m_arr[10], M[10])
			&& closeEnough(m_arr[11], M[11])
			&& closeEnough(m_arr[12], M[12])
			&& closeEnough(m_arr[13], M[13])
			&& closeEnough(m_arr[14], M[14])
			&& closeEnough(m_arr[15], M[15]);
	}

	bool operator!=(const mat4 &M) const
	{
		return !(*this == M);
	}

	mat4 & operator+=(const mat4 &M)
	{
		m_arr[ 0] += M[ 0]; m_arr[ 1] += M[ 1]; m_arr[ 2] += M[ 2]; m_arr[ 3] += M[ 3];
		m_arr[ 4] += M[ 4]; m_arr[ 5] += M[ 5]; m_arr[ 6] += M[ 6]; m_arr[ 7] += M[ 7];
		m_arr[ 8] += M[ 8]; m_arr[ 9] += M[ 9]; m_arr[10] += M[10]; m_arr[11] += M[11];
		m_arr[12] += M[12]; m_arr[13] += M[13]; m_arr[14] += M[14]; m_arr[15] += M[15];
		return *this;
	}

	mat4 & operator-=(const mat4 &M)
	{
		m_arr[ 0] -= M[ 0]; m_arr[ 1] -= M[ 1]; m_arr[ 2] -= M[ 2]; m_arr[ 3] -= M[ 3];
		m_arr[ 4] -= M[ 4]; m_arr[ 5] -= M[ 5]; m_arr[ 6] -= M[ 6]; m_arr[ 7] -= M[ 7];
		m_arr[ 8] -= M[ 8]; m_arr[ 9] -= M[ 9]; m_arr[10] -= M[10]; m_arr[11] -= M[11];
		m_arr[12] -= M[12]; m_arr[13] -= M[13]; m_arr[14] -= M[14]; m_arr[15] -= M[15];
		return *this;
	}

	mat4 & operator*=(const mat4 &M)	// TODO: check!!!
	{
		m_arr[ 0] *= (m_arr[ 0] * M[ 0] + m_arr[ 1] * M[ 4] + m_arr[ 2] * M[ 8] + m_arr[ 3] * M[12]);
		m_arr[ 0] *= (m_arr[ 0] * M[ 1] + m_arr[ 1] * M[ 5] + m_arr[ 2] * M[ 9] + m_arr[ 3] * M[13]);
		m_arr[ 0] *= (m_arr[ 0] * M[ 2] + m_arr[ 1] * M[ 6] + m_arr[ 2] * M[10] + m_arr[ 3] * M[14]);
		m_arr[ 0] *= (m_arr[ 0] * M[ 3] + m_arr[ 1] * M[ 7] + m_arr[ 2] * M[11] + m_arr[ 3] * M[15]);
		m_arr[ 0] *= (m_arr[ 4] * M[ 0] + m_arr[ 5] * M[ 4] + m_arr[ 6] * M[ 8] + m_arr[ 7] * M[12]);
		m_arr[ 0] *= (m_arr[ 4] * M[ 1] + m_arr[ 5] * M[ 5] + m_arr[ 6] * M[ 9] + m_arr[ 7] * M[13]);
		m_arr[ 0] *= (m_arr[ 4] * M[ 2] + m_arr[ 5] * M[ 6] + m_arr[ 6] * M[10] + m_arr[ 7] * M[14]);
		m_arr[ 0] *= (m_arr[ 4] * M[ 3] + m_arr[ 5] * M[ 7] + m_arr[ 6] * M[11] + m_arr[ 7] * M[15]);
		m_arr[ 0] *= (m_arr[ 8] * M[ 0] + m_arr[ 9] * M[ 4] + m_arr[10] * M[ 8] + m_arr[11] * M[12]);
		m_arr[ 0] *= (m_arr[ 8] * M[ 1] + m_arr[ 9] * M[ 5] + m_arr[10] * M[ 9] + m_arr[11] * M[13]);
		m_arr[ 0] *= (m_arr[ 8] * M[ 2] + m_arr[ 9] * M[ 6] + m_arr[10] * M[10] + m_arr[11] * M[14]);
		m_arr[ 0] *= (m_arr[ 8] * M[ 3] + m_arr[ 9] * M[ 7] + m_arr[10] * M[11] + m_arr[11] * M[15]);
		m_arr[ 0] *= (m_arr[12] * M[ 0] + m_arr[13] * M[ 4] + m_arr[14] * M[ 8] + m_arr[15] * M[12]);
		m_arr[ 0] *= (m_arr[12] * M[ 1] + m_arr[13] * M[ 5] + m_arr[14] * M[ 9] + m_arr[15] * M[13]);
		m_arr[ 0] *= (m_arr[12] * M[ 2] + m_arr[13] * M[ 6] + m_arr[14] * M[10] + m_arr[15] * M[14]);
		m_arr[ 0] *= (m_arr[12] * M[ 3] + m_arr[13] * M[ 7] + m_arr[14] * M[11] + m_arr[15] * M[15]);
		return *this;
	}

	mat4 & operator*=(float f)
	{ 
		m_arr[ 0] *= f; m_arr[ 1] *= f; m_arr[ 2] *= f; m_arr[ 3] *= f;
		m_arr[ 4] *= f; m_arr[ 5] *= f; m_arr[ 6] *= f; m_arr[ 7] *= f;
		m_arr[ 8] *= f; m_arr[ 9] *= f; m_arr[10] *= f; m_arr[11] *= f;
		m_arr[12] *= f; m_arr[13] *= f; m_arr[14] *= f; m_arr[15] *= f;
		return *this;
	}

	mat4 & operator/=(float f)
	{ 
		m_arr[ 0] /= f; m_arr[ 1] /= f; m_arr[ 2] /= f; m_arr[ 3] /= f;
		m_arr[ 4] /= f; m_arr[ 5] /= f; m_arr[ 6] /= f; m_arr[ 7] /= f;
		m_arr[ 8] /= f; m_arr[ 9] /= f; m_arr[10] /= f; m_arr[11] /= f;
		m_arr[12] /= f; m_arr[13] /= f; m_arr[14] /= f; m_arr[15] /= f;
		return *this;
	}

	const mat4 operator+(const mat4 &M) const
	{ 
		return mat4(m_arr[ 0] + M[ 0], m_arr[ 1] + M[ 1], m_arr[ 2] + M[ 2], m_arr[ 3] + M[ 3],
					m_arr[ 4] + M[ 4], m_arr[ 5] + M[ 5], m_arr[ 6] + M[ 6], m_arr[ 7] + M[ 7],
					m_arr[ 8] + M[ 8], m_arr[ 9] + M[ 9], m_arr[10] + M[10], m_arr[11] + M[11],
					m_arr[12] + M[12], m_arr[13] + M[13], m_arr[14] + M[14], m_arr[15] + M[15]);
	}

	const mat4 operator-(const mat4 &M) const
	{
		return mat4(m_arr[ 0] - M[ 0], m_arr[ 1] - M[ 1], m_arr[ 2] - M[ 2], m_arr[ 3] - M[ 3],
					m_arr[ 4] - M[ 4], m_arr[ 5] - M[ 5], m_arr[ 6] - M[ 6], m_arr[ 7] - M[ 7],
					m_arr[ 8] - M[ 8], m_arr[ 9] - M[ 9], m_arr[10] - M[10], m_arr[11] - M[11],
					m_arr[12] - M[12], m_arr[13] - M[13], m_arr[14] - M[14], m_arr[15] - M[15]);
	}

	const mat4 operator*(const mat4 &M) const
	{
		return mat4(m_arr[ 0] * M[ 0] + m_arr[ 1] * M[ 4] + m_arr[ 2] * M[ 8] + m_arr[ 3] * M[12],
					m_arr[ 0] * M[ 1] + m_arr[ 1] * M[ 5] + m_arr[ 2] * M[ 9] + m_arr[ 3] * M[13],
					m_arr[ 0] * M[ 2] + m_arr[ 1] * M[ 6] + m_arr[ 2] * M[10] + m_arr[ 3] * M[14],
					m_arr[ 0] * M[ 3] + m_arr[ 1] * M[ 7] + m_arr[ 2] * M[11] + m_arr[ 3] * M[15],
					m_arr[ 4] * M[ 0] + m_arr[ 5] * M[ 4] + m_arr[ 6] * M[ 8] + m_arr[ 7] * M[12],
					m_arr[ 4] * M[ 1] + m_arr[ 5] * M[ 5] + m_arr[ 6] * M[ 9] + m_arr[ 7] * M[13],
					m_arr[ 4] * M[ 2] + m_arr[ 5] * M[ 6] + m_arr[ 6] * M[10] + m_arr[ 7] * M[14],
					m_arr[ 4] * M[ 3] + m_arr[ 5] * M[ 7] + m_arr[ 6] * M[11] + m_arr[ 7] * M[15],
					m_arr[ 8] * M[ 0] + m_arr[ 9] * M[ 4] + m_arr[10] * M[ 8] + m_arr[11] * M[12],
					m_arr[ 8] * M[ 1] + m_arr[ 9] * M[ 5] + m_arr[10] * M[ 9] + m_arr[11] * M[13],
					m_arr[ 8] * M[ 2] + m_arr[ 9] * M[ 6] + m_arr[10] * M[10] + m_arr[11] * M[14],
					m_arr[ 8] * M[ 3] + m_arr[ 9] * M[ 7] + m_arr[10] * M[11] + m_arr[11] * M[15],
					m_arr[12] * M[ 0] + m_arr[13] * M[ 4] + m_arr[14] * M[ 8] + m_arr[15] * M[12],
					m_arr[12] * M[ 1] + m_arr[13] * M[ 5] + m_arr[14] * M[ 9] + m_arr[15] * M[13],
					m_arr[12] * M[ 2] + m_arr[13] * M[ 6] + m_arr[14] * M[10] + m_arr[15] * M[14],
					m_arr[12] * M[ 3] + m_arr[13] * M[ 7] + m_arr[14] * M[11] + m_arr[15] * M[15]);
	}

	const mat4 operator*(float f) const
	{
		return mat4(m_arr[ 0] * f, m_arr[ 1] * f, m_arr[ 2] * f, m_arr[ 3] * f,
					m_arr[ 4] * f, m_arr[ 5] * f, m_arr[ 6] * f, m_arr[ 7] * f,
					m_arr[ 8] * f, m_arr[ 9] * f, m_arr[10] * f, m_arr[11] * f,
					m_arr[12] * f, m_arr[13] * f, m_arr[14] * f, m_arr[15] * f);
	}

	const mat4 operator/(float f) const
	{
		return mat4(m_arr[ 0] / f, m_arr[ 1] / f, m_arr[ 2] / f, m_arr[ 3] / f,
					m_arr[ 4] / f, m_arr[ 5] / f, m_arr[ 6] / f, m_arr[ 7] / f,
					m_arr[ 8] / f, m_arr[ 9] / f, m_arr[10] / f, m_arr[11] / f,
					m_arr[12] / f, m_arr[13] / f, m_arr[14] / f, m_arr[15] / f);
	}

	float operator[](int i)       
	{ 
		return m_arr[i]; 
	}

	float operator[](int i) const 
	{ 
		return m_arr[i]; 
	}

	void operator()(const mat4 &M)
	{ 
		memcpy(m_arr, M.m_arr, sizeof(float) * 16);
	}
};

inline const vec2 operator*(const mat4 &M, const vec2 &v)
{
	return vec2(M[0] * v.x + M[1] * v.y + M[2] + M[3],
				M[4] * v.x + M[5] * v.y + M[6] + M[7]);
}

inline const vec3 operator*(const mat4 &M, const vec3 &v)
{
	return vec3(M[ 0] * v.x + M[ 1] * v.y + M[ 2] * v.z + M[ 3],
				M[ 4] * v.x + M[ 5] * v.y + M[ 6] * v.z + M[ 7],
				M[ 8] * v.x + M[ 9] * v.y + M[10] * v.z + M[11]);
}

inline const vec4 operator*(const mat4 &M, const vec4 &v)
{
	return vec4(M[ 0] * v.x + M[ 1] * v.y + M[ 2] * v.z + M[ 3] * v.w,
				M[ 4] * v.x + M[ 5] * v.y + M[ 6] * v.z + M[ 7] * v.w,
				M[ 8] * v.x + M[ 9] * v.y + M[10] * v.z + M[11] * v.w,
				M[12] * v.x + M[13] * v.y + M[14] * v.z + M[15] * v.w);
}

inline mat3::mat3(const mat4 &M)  
	: a0(M[0]), a1(M[1]), a2(M[ 2]), 
	  a3(M[4]), a4(M[5]), a5(M[ 6]), 
	  a6(M[8]), a7(M[9]), a8(M[10]) 
{}
//-----------------------------------------------------------------------------
// This Quaternion class will concatenate quaternions in a left to right order.
// The reason for this is to maintain the same multiplication semantics as the
// Matrix3 and Matrix4 classes.

//class Quaternion
//{
//	friend Quaternion operator*(float lhs, const Quaternion &rhs);
//
//public:
//	static const Quaternion IDENTITY;
//
//	float w, x, y, z;
//
//	static Quaternion slerp(const Quaternion &a, const Quaternion &b, float t);
//
//	Quaternion() {}
//	Quaternion(float w_, float x_, float y_, float z_);
//	Quaternion(float headDegrees, float pitchDegrees, float rollDegrees);
//	Quaternion(const vec3 &axis, float degrees);
//	explicit Quaternion(const mat3 &m);
//	explicit Quaternion(const mat4 &m);
//	~Quaternion() {}
//
//	bool operator==(const Quaternion &rhs) const;
//	bool operator!=(const Quaternion &rhs) const;
//
//	Quaternion &operator+=(const Quaternion &rhs);
//	Quaternion &operator-=(const Quaternion &rhs);
//	Quaternion &operator*=(const Quaternion &rhs);
//	Quaternion &operator*=(float scalar);
//	Quaternion &operator/=(float scalar);
//
//	Quaternion operator+(const Quaternion &rhs) const;
//	Quaternion operator-(const Quaternion &rhs) const;
//	Quaternion operator*(const Quaternion &rhs) const;
//	Quaternion operator*(float scalar) const;
//	Quaternion operator/(float scalar) const;
//
//	Quaternion conjugate() const;
//	void fromAxisAngle(const vec3 &axis, float degrees);
//	void fromHeadPitchRoll(float headDegrees, float pitchDegrees, float rollDegrees);
//	void fromMatrix(const mat3 &m);
//	void fromMatrix(const mat4 &m);
//	void identity();
//	Quaternion inverse() const;
//	float magnitude() const;
//	void normalize();
//	void set(float w_, float x_, float y_, float z_);
//	void toAxisAngle(vec3 &axis, float &degrees) const;
//	void toHeadPitchRoll(float &headDegrees, float &pitchDegrees, float &rollDegrees) const;
//	mat3 toMatrix3() const;
//	mat4 toMatrix4() const;
//};

//=========================================================================== 
//									CONSTANTS
//=========================================================================== 

const vec2 vec2_zero(0, 0);

const vec2 vec2_x(1, 0);

const vec2 vec2_y(0, 1);

const vec3 vec3_zero(0, 0, 0);

const vec3 vec3_x(1, 0, 0);

const vec3 vec3_y(0, 1, 0);

const vec3 vec3_z(0, 0, 1);

const mat3 mat3_zero(0, 0, 0,
					 0, 0, 0,
					 0, 0, 0);

const mat3 mat3_identity(1, 0, 0,
						 0, 1, 0,
						 0, 0, 1);

const mat4 mat4_zero(0, 0, 0, 0,
					 0, 0, 0, 0,
					 0, 0, 0, 0,
					 0, 0, 0, 0);

const mat4 mat4_identity(1, 0, 0, 0,
						 0, 1, 0, 0,
						 0, 0, 1, 0,
						 0, 0, 0, 1);
